/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : motor.cpp
  * Description        :
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Oct 23, 2020
  ******************************************************************************
  */
/* USER CODE END Header */

#include "motor.hpp"

MOTOR motor(TIM3,TIM4,(char *)"MOTOR");

void MOTOR::motor_Init(void)
{

	p_limits=(CS_LIMIT_MAX/RAD_TO_PWM)*180.0f/3.1415926535897931f;//单位：度
	//--------------------------------------------
	Update = true;
	Sta = STA_INI;
	Err = ERR_NONE;

	for(u8 i=0;i<PWM_OUT_NUM;i++)
	{
		PWM[i] = INI_PWM;
		PwmOff[i] = 0;
	}

	osDelay(100);

	//启动定时器

	LL_TIM_EnableCounter(htimA);
	LL_TIM_OC_SetCompareCH1(htimA,PWM[0]);
	LL_TIM_OC_SetCompareCH2(htimA,PWM[1]);
	LL_TIM_OC_SetCompareCH3(htimA,PWM[2]);
	LL_TIM_OC_SetCompareCH4(htimA,PWM[3]);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH1);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH2);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH3);
	LL_TIM_CC_EnableChannel(htimA, LL_TIM_CHANNEL_CH4);
	LL_TIM_EnableAllOutputs(htimA);


	LL_TIM_EnableCounter(htimB);
	LL_TIM_OC_SetCompareCH1(htimB, PWM[4]);
	LL_TIM_OC_SetCompareCH2(htimB, PWM[5]);
	LL_TIM_OC_SetCompareCH3(htimB, PWM[6]);
	LL_TIM_OC_SetCompareCH4(htimB, PWM[7]);
	LL_TIM_CC_EnableChannel(htimB, LL_TIM_CHANNEL_CH1);
	LL_TIM_CC_EnableChannel(htimB, LL_TIM_CHANNEL_CH2);
	LL_TIM_CC_EnableChannel(htimB, LL_TIM_CHANNEL_CH3);
	LL_TIM_CC_EnableChannel(htimB, LL_TIM_CHANNEL_CH4);
	LL_TIM_EnableAllOutputs(htimB);

}

void MOTOR::motor_run(void)
{
	xQueuePeek(queueRCCommand, &rcCommand, 0);					//从队列中获取遥控器数据
	xQueuePeek(queueControlOutputDat, &control_output, 0);					//从队列中获取遥控器数据
	xQueuePeek(queuePID, &pid, 0);

	if(rcCommand.Mode == RC_MOT_UNLOCK)UnLock = true;
	else if(rcCommand.Mode == RC_MOT_LOCK)UnLock = false;
	if(UnLock == false)    //电机锁定
	{
		for(u8 i=0;i<PWM_OUT_NUM;i++)
			PWM[i]=INI_PWM;
	}

//	PWM[0] = 1500-rcCommand.Val[3];
	PWM[0] = iConstrain(control_output.mt_output[0],1000,2000);
	PWM[1] = iConstrain(PWM_MID_1+control_output.cs_output[0],PWM_MID_1+CS_LIMIT_MIN,PWM_MID_1+CS_LIMIT_MAX);
	PWM[2] = iConstrain(PWM_MID_2+control_output.cs_output[1],PWM_MID_2+CS_LIMIT_MIN,PWM_MID_2+CS_LIMIT_MAX);
  	PWM[3] = iConstrain(PWM_MID_3+control_output.cs_output[2],PWM_MID_3+CS_LIMIT_MIN,PWM_MID_3+CS_LIMIT_MAX);
  	PWM[4] = iConstrain(PWM_MID_4+control_output.cs_output[3],PWM_MID_4+CS_LIMIT_MIN,PWM_MID_4+CS_LIMIT_MAX);

//	PWM[4] = pid.ki[12]*1000;

	LL_GPIO_ResetOutputPin(GPIOB,GPIO_PIN_0);

	for(u8 i=0;i<PWM_OUT_NUM;i++)  //限幅
		motor_msg.PWM_OBS[i]=PWM[i]=iConstrain(PWM[i]-PwmOff[i],Min_PWM_Out,Max_PWM_Out);

	LL_TIM_OC_SetCompareCH1(htimA, PWM[0]);
	LL_TIM_OC_SetCompareCH2(htimA, PWM[1]);
	LL_TIM_OC_SetCompareCH3(htimA, PWM[2]);
	LL_TIM_OC_SetCompareCH4(htimA, PWM[3]);

	LL_TIM_OC_SetCompareCH1(htimB, PWM[4]);
	LL_TIM_OC_SetCompareCH2(htimB, PWM[5]);
	LL_TIM_OC_SetCompareCH3(htimB, PWM[6]);
	LL_TIM_OC_SetCompareCH4(htimB, PWM[7]);

	xQueueOverwrite(queueMotorData,&motor_msg);

}


extern "C" void motor_main(void *argument)
{
	motor.motor_Init();
	osDelay(1);
	for(;;)
	{
		osSemaphoreAcquire(semMotor,0xffffffff);
		motor.motor_run();
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
